#include "pscb.h"

#include <string.h>
#include <thread>
#include <chrono>
#include <string>

#include "rtdev/rtdev.h"
#include "rtbus/rtbus.h"
#include "jkutil/configparser.h"
#include "jkutil/bitdataop.h"
#include "log/zlog.hh"
#include "hwdrv/hwdrv.h"

#define PSCB_ERR(fmt, ...) zlog()->error("[PSCB] " fmt, ##__VA_ARGS__)
#define PSCB_INFO(fmt, ...) zlog()->info("[PSCB] " fmt, ##__VA_ARGS__)

using rtdev::PScb;
using rtdev::PScbCab;
using rtdev::PScbFactory;
using rtdev::SafetyBoardItf;
using rtdev::ScbRobPowerItf;

PScb::PScb()
{
    bus_.reset();
    sim_flag_ = 0;
    cab_.scb = this;
    rob.scb = this;
}

int PScb::init(void* info, int sim)
{
    //根据配置信息找到总线
    auto bus = rtbus::BusMgr::get()->bus("scbbus");
    if (bus.get() == nullptr)
    {
        return -1;
    }
    bus_ = bus;

    sim_flag_ = sim;
    PSCB_INFO("PSCB init success.bus:{} sim:{}", bus_->get_name(), is_sim());
    return 0;
}

int PScb::boot()
{
    if (!bus_.get())
        return -1;
    if (!is_sim())
    {
        bus_->config_txpdo(0x212, 8);  // do
        bus_->config_txpdo(0x293, 8);  // safety do
        bus_->config_txpdo(0x312, 8);  // ao

        bus_->config_rxpdo(0x192, 8);
        bus_->config_rxpdo(0x193, 8);
        bus_->config_rxpdo(0x292, 8);
        bus_->config_rxpdo(0x298, 8);
        bus_->register_sdo_handle(0x12, 0x612, 0x612, 0x592, 3);

        int err = get_sdo_blocking(0x214800, &cab_.stat.hw_ver, 2, 40);
        if (err)
        {
            PSCB_ERR("get_sdo_blocking 0x214800 failed!");
            return err;
        }
        uint16_t sw_ver_h = 0;
        uint16_t sw_ver_l = 0;
        get_sdo_blocking(0x214900, &sw_ver_h, 2, 40);
        get_sdo_blocking(0x214a00, &sw_ver_l, 2, 40);
        cab_.stat.sw_ver = (sw_ver_h << 16) | sw_ver_l;

        uint16_t ver_stat = 0;
        get_sdo_blocking(0x214800, &ver_stat, 2, 40);
        cab_.stat.version_status = ver_stat & 0xff;
    }
    return 0;
}

int PScb::set_send_data(int busid)
{
    check_alive();
    uint8_t data[8] = {0};
    Util::set_bit(data + 2, 0, 0);
    Util::set_bit(data + 2, 1, 1);                         // 手柄电源控制
    Util::set_bit(data + 2, 2, 0);                         // 控制器请求刷新手柄固件
    Util::set_bit(data + 2, 3, 0);                         // 控制器完成刷新安全板
    Util::set_bit(data + 2, 4, 0);                         // 控制器请求刷新安全板
    Util::set_bit(data + 2, 5, cab_.cmd.dis_robot_commu);  // 请求伺服驱动刷机
    Util::set_bit(data + 2, 6, cab_.cmd.pwr_ipc);          // 请求关机
    Util::set_bit(data + 2, 7, rob.is_cmd_power_on);       // 机器人本体电源控制
    bus_->set_txpdo_data(0x212, data);

    memset(data, 0, 8);
    bus_->set_txpdo_data(0x293, data);

    memset(data, 0, 8);
    data[0] = 0x00;                                    //数字IO配置
    data[1] = cab_.cmd_stuck_voltage & 0xff;           //泄放电压配置
    data[2] = (cab_.cmd_stuck_voltage & 0xff00) >> 8;  //泄放电压配置
    data[3] = cab_.cmd.front_pnal_rgb_ctl;             //面板灯控制
    bus_->set_txpdo_data(0x312, data);
    return 0;
}
int PScb::get_recv_data(int busid)
{
    uint8_t data[8] = {0};
    auto alive_cnt = bus_->get_rxpdo_data(0x192, data);
    if (keep_alive(alive_cnt))
    {
        return -1;
    }

    safedev.si[0] = Util::get_bit(&data[2], 2);
    safedev.si[1] = Util::get_bit(&data[2], 3);
    safedev.si_fault = false;
    safedev.ext_ei[0] = Util::get_bit(&data[2], 4);
    safedev.ext_ei[1] = Util::get_bit(&data[2], 5);
    safedev.ext_ei_fault = false;
    safedev.ei[0] = Util::get_bit(&data[2], 6);
    safedev.ei[1] = Util::get_bit(&data[2], 7);
    safedev.ei_fault = false;

    bus_->get_rxpdo_data(0x192, data);
    bus_->get_rxpdo_data(0x193, data);
    bus_->get_rxpdo_data(0x292, data);
    bus_->get_rxpdo_data(0x298, data);

    return 0;
}
int PScb::get_sdo_blocking(uint32_t objid, void* data, int s, int retry) { return bus_->get_sdo_blocking(0x12, objid, data, s, retry); }
int PScb::set_sdo_blocking(uint32_t objid, void* data, size_t s, void* readback, int retry)
{
    return bus_->set_sdo_blocking(0x12, objid, data, s, readback, retry);
}
rtdev::SafetyBoardItf* rtdev::PScbFactory::create() { return new PScb(); }

PScbCab::PScbCab()
{
    cmd_cab_id = act_cab_id = 0;
    cmd_stuck_voltage = act_stuck_voltage = 51;
    scb = nullptr;
}

int PScbCab::get_id()
{
    if (scb->is_sim())
    {
        return 0;
    }
    return act_cab_id;
}

int PScbCab::set_id(uint32_t id)
{
    if (scb->is_sim())
        return 0;
    cmd_cab_id = id;
    return scb->set_sdo_blocking(0x314e00, &id, 4, &act_cab_id, 3);
}

int PScbCab::get_stuck_voltage(bool is_act) { return is_act ? act_stuck_voltage : cmd_stuck_voltage; }

int PScbCab::set_stuck_voltage(int voltage)
{
    scb->set_sdo_blocking(0x314f00, &voltage, 2);
    return 0;
}

int PScbCab::set_power_supply(int val)
{
    // pscb固件不支持，直接返回-1
    return -1;
}

int PScbCab::get_power_supply()
{
    // pscb固件不支持，直接返回24V
    return 24;
}

rtdev::PSCBSafetySignal::PSCBSafetySignal()
{
    ei[0] = ei[1] = true;
    ei_fault = false;
    ext_ei[0] = ext_ei[1] = true;
    ext_ei_fault = false;
    si[0] = si[1] = true;
    si_fault = false;
}

int rtdev::PSCBSafetySignal::get_ei_stat(bool dual_chn[2], bool* has_fault)
{
    dual_chn[0] = ei[0];
    dual_chn[1] = ei[1];
    if (has_fault)
        *has_fault = ei_fault;
    return 0;
}
int rtdev::PSCBSafetySignal::get_ext_ei_stat(bool dual_chn[2], bool* has_fault)
{
    dual_chn[0] = ext_ei[0];
    dual_chn[1] = ext_ei[1];
    if (has_fault)
        *has_fault = ext_ei_fault;
    return 0;
}
int rtdev::PSCBSafetySignal::get_si_stat(bool dual_chn[2], bool* has_fault)
{
    dual_chn[0] = si[0];
    dual_chn[1] = si[1];
    if (has_fault)
        *has_fault = si_fault;
    return 0;
}

int rtdev::PSCBSafetySignal::set_tcp_offset(float x, float y, float z) { return 0; }
